int velocidadNormal=70;

void mover(int velocidad,int distancia, int direccion)// Distancia en centimetros,0 adelante, diferente a cero atras, velocidad 0 a 100
{
    nSyncedMotors= synchAC;                                  // "A" va a ser sincronizado a "C".
    nSyncedTurnRatio= 100;                                  // linea recta 79
    nMotorEncoderTarget[motorA]  = 360*distancia/10;       // 360=10cm  incremental amount to move motor -- one revolution
    motor[motorA]= (direccion==0)?-velocidad:velocidad;   // motor speed
    while (nMotorRunState[motorA] != runStateIdle)
    {}   																						// wait for action to complete
    wait1Msec(500);

}


void girar90grados(int direccion)// 0 izquierda, direfente a cero derecha
{
    if (direccion==0){
                                                             // Giro izquierda, motorC-->izquierda, motorA--> derecha
        nSyncedMotors= synchCA;                             // "C" will be synchronized to "A".
	nSyncedTurnRatio= -100;                            // Reversed
        nMotorEncoderTarget[motorC] = 360*2;              // incremental amount to move motor -- one revolution*n 
        motor[motorC]= 50;                               // motor speed
        while (nMotorRunState[motorC] != runStateIdle)
        {}  																						// wait for action to complete																				// wait for action to complete
        wait1Msec(500);
     }
     else{
        nSyncedMotors= synchAC;                              // "A" will be synchronized to "C".
	nSyncedTurnRatio= -100;                             // Reversed
        nMotorEncoderTarget[motorA] = 360*1.8;             // incremental amount to move motor -- one revolution
        motor[motorA]= 50;                                // motor speed
        while (nMotorRunState[motorA] != runStateIdle)
        {}  																						// wait for action to complete
        wait1Msec(2000);																// a little delay between actions
     }

}

void girar180grados()
{
   girar90grados(0);
   girar90grados(0);

}

void irA(int origen, int destino, int velocidad)
{
   if (origen == 8){
        if (destino==17){
           mover(velocidadNormal,260,0);
           girar90grados(0);
           mover(velocidadNormal,100,0);
        }
        else if(destino==15){
           mover(velocidadNormal,260,0);
           girar90grados(1);
           mover(velocidadNormal,100,0);
        }
        else if(destino==14){
           mover(velocidadNormal,183,0);
           girar90grados(0);
           mover(velocidadNormal,100,0);
        }
        else if (destino==12){
           mover(velocidadNormal,183,0);
           girar90grados(1);
           mover(velocidadNormal,100,0);
        }
	else if (destino==9){
	   girar90grados(1);
           mover(velocidadNormal,94,0);
        }
        else if (destino==7){
           girar90grados(0);
           mover(velocidadNormal,94,0);
        }
        else if (destino==6){
           mover(velocidadNormal,68,0);
           girar90grados(1);
           mover(velocidadNormal,68,0);
        }
        else if (destino==5)
           mover(velocidadNormal,68,0);
        else if (destino==4){
           mover(velocidadNormal,68,0);
           girar90grados(0);
           mover(velocidadNormal,68,0);
        }
        else if (destino==3){
           mover(velocidadNormal,68,0);
           girar90grados(1);
           mover(velocidadNormal,68,0);
           girar90grados(0);
           mover(velocidadNormal,68,0);
        }
        else if(destino==2)
           mover(velocidadNormal,136,0);
        else if(destino==1){
           mover(velocidadNormal,68,0);
           girar90grados(0);
           mover(velocidadNormal,68,0);
           girar90grados(1);
           mover(velocidadNormal,68,0);
        }
    else if(destino==8){
        if (origen==17){
           mover(velocidadNormal,100,0);
           girar90grados(1);
           mover(velocidadNormal,260,0);
        }
        else if(origen==15){
           mover(velocidadNormal,100,0);
           girar90grados(0);
           mover(velocidadNormal,260,0);
        }
        else if(origen==14){
           mover(velocidadNormal,100,0);
           girar90grados(1);
           mover(velocidadNormal,183,0);
        }
        else if (origen==12){
           mover(velocidadNormal,100,0);
           girar90grados(0);
           mover(velocidadNormal,183,0);
        }
	else if (origen==9){
	   mover(velocidadNormal,94,0);
        }
        else if (origen==7){
           mover(velocidadNormal,94,0);
        }
        else if (origen==6){
           mover(velocidadNormal,68,0);
           girar90grados(0);
           mover(velocidadNormal,68,0);
        }
        else if (origen==5)
           mover(velocidadNormal,68,0);
        else if (origen==4){
           mover(velocidadNormal,68,0);
           girar90grados(1);
           mover(velocidadNormal,68,0);
        }
        else if (origen==3){
           mover(velocidadNormal,68,0);
           girar90grados(1);
           mover(velocidadNormal,68,0);
           girar90grados(0);
           mover(velocidadNormal,68,0);
        }
        else if(origen==2)
           mover(velocidadNormal,136,0);
        else if(origen==1){
           mover(velocidadNormal,68,0);
           girar90grados(0);
           mover(velocidadNormal,68,0);
           girar90grados(1);
           mover(velocidadNormal,68,0);
        }
    }
}



